Robert H . Bishop , and Daniele Mortari Norm - Constrained Kalman Filtering

The problem of estimating the state vector of a dynamical system from vector measurements when it is known that the state vector satisfies norm equality constraints is considered. The case of a linear dynamical system with linear measurements subject to a norm equality constraint is discussed with a review of existing solutions. The norm constraint introduces a nonlinearity in the system for which a new estimator structure is derived by minimizing a constrained cost function. It is shown that the constrained estimate is equivalent to the brute force normalization of the unconstrained estimate. The obtained solution is extended to nonlinear measurement models and applied to the spacecraW attitude filtering problems. Introduction It is well-known that the Kalman filter provides the unconstrained optimal solution of the linear stochastic estimation problem [1],[2]. The Kalman filter algorithm has two main phases: the state estimate propagation phase between measurements and the state estimate update phase when measurements become available. Unconstrained implies that the optimal state estimate is not constrained during the state estimate update phase as the measurements are processed. The Kalman filter provides the optimal state estimate considering n degrees of freedom (that is, the entire vector space ). However, if r state constraints are applied, the degrees of freedom are reduced to n − r. Projecting the unconstrained solution into the constrained space will not guarantee optimality. This work focuses on norm constraints applied to the state vector. It is assumed throughout this paper that through the mathematical model (that is, through the state equation), the underlying physics, including the state constraints, are satisfied during periods between measurements. The mathematical model of the system should adequately represent any desired state constraints. The objective of this work is to modify the Kalman filter solution to constrain the state update appropriately. In the discrete formulation of the Kalman filter, the state can be related to the control algebraically. The optimization problem is formulated as a parameter optimization problem; therefore, the state constraint can be expressed as a control constraint. A motivation to seek the norm-constrained solution to the filtering problem is attitude estimation. Attitude estimation has been the topic of much research and debate in the past two decades [3]. The interest arises from the fact that the representation of the attitude is not a vector space and redundancy is necessary to avoid singularities and discontinuities [4]. For real-time space applications, the quaternionof-rotation is the preferred attitude representation. In order to represent a rotation, the quaternion obeys a unitnorm constraint. This work will develop the theoretical foundations of norm-constrained Kalman filtering with reference to the quaternion estimation problem. One method of introducing state constraints is to use pseudomeasurements [5]. The fundamental idea is to introduce a perfect measurement (hence the use of the term “pseudomeasurement”) consisting of the constraint equation into the estimation solution. This approach has shortcomings. The use of a perfect measurement results in a singular estimation problem known to occur when processing noise-free measurements in a Kalman filter. A *Presented as paper AIAA 2006-6164 at the 2006 AIAA/AAS Astrodynamics Specialist Conference and paper AAS 08-215 at the 2008 AAS/AIAA Space Flight Mechanics Meeting. Norm-Constrained Kalman Filtering 33 small noise can be added to the pseudomeasurement to address the singularity; however, with the noise introduced, the constraint is no longer exactly satisfied. One can consider state constraints when considering the optimization problems based on least squares methods. The solution to the least squares problem in the presence of linear equality constraints is found in Lawson and Hanson [6]. Another approach is to project the Kalman solution into the desired subspace. Since the projection can be done in different ways, a performance index can be defined to find the optimal projection. The optimal projection for the linear state equality constraint problem is presented in Simon and Chia [7]. The projection of the Kalman solution can be done at any time, not only during the update. The constrained quaternion estimation problem was posed as a nonlinear programming problem by Psiaki [8] and solved using Newton’s method. Psiaki’s approach differs from the proposed approach in several key areas. First, Psiaki minimizes a different cost function and his method has a different interpretation of the covariance. Psiaki’s method is a global optimal that solves a quadratically constrained quadratic program at every update stage that will work with poor or no initial estimate. The method proposed here is a prediction correction technique that relies on a priori estimates. In sequential real-time quaternion estimation, two main approaches received the most attention: the Additive Extended Kalman Filter (AEKF) [9] and the Multiplicative Extended Kalman Filter (MEKF) [10]. Both the AEKF and MEKF necessitate restoring the norm constraint aWer the update. The straightforward method is to scale the updated quaternion by its norm, thereby minimizing the Euclidean distance between the unconstrained and the constrained estimates [11]. The main focus of this work is to obtain the optimal estimate while simultaneously constraining the norm. The result is that the normalization process provides the unitary estimate with minimum mean square error–a fact heretofore unproven. Previous work on the AEKF assumed quaternion normalization and studied the consequences [9],[12],[13]. In this work, normalization is not assumed, but is a direct result of the optimization process. The paper is organized as follows. “Norm-Constrained Kalman Filtering” develops the new filter for a general norm constraint assuming linear dynamics and a linear measurement model. The problem is nonlinear because of the quadratic norm constraint. “Constrain Only Part of the State” shows how a subset of the state vector can be estimated using the norm-constrained algorithm. “Attitude Estimation” details the quaternion estimation problem used for the numerical examples. The results of “NormConstrained Kalman Filtering” are extended to a nonlinear measurement model for this example. “Numerical Results” contains numerical simulations of the new algorithm applied to quaternion estimation. “Conclusions” summarizes the work and develops some conclusions.